/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:08:57
 * @LastEditTime: 2021-10-27 14:11:36
 * @LastEditors: Sang Hao
 */
#include "lidar_slam/models/cloud_filter/voxel_filter.hpp"

namespace lidar_slam {
bool VoxelFilter::SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z) {
	voxel_filter_.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z);
	return true;
}

VoxelFilter::VoxelFilter(const YAML::Node& config_node) {
	float leaf_size_x = config_node["leaf_size"][0].as<float>();
	float leaf_size_y = config_node["leaf_size"][1].as<float>();
	float leaf_size_z = config_node["leaf_size"][2].as<float>();
	SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z);
}

VoxelFilter::VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z) {
	SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z);
}

bool VoxelFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, 
		CloudData::CLOUD_PTR& filter_cloud_ptr) {
	voxel_filter_.setInputCloud(input_cloud_ptr);
	voxel_filter_.filter(*filter_cloud_ptr);
	
	return true;
}
}